#include "troyframe.h"

#include "Core/Robot/nullbot.h"

TroyFrame::TroyFrame(QWidget *parent) :
    QFrame(parent),
    _controller(new NullGLController()),
    _model(new Troy()),
    _view(new TroyGLView(this)),
    _action(NULL),
    _ownsAction(false),
    _troyScale(1),
    _mirrored(false),
    _mirrorLabel(new QLabel("Mirrored", this))
{
    _controller->loadModel(_model);
    _controller->loadView(_view);

    _view->loadController(_controller);
    _view->loadModel(_model);
    setMinimumSize(10, 10);
    //_view->setMinimumSize(10, 10);
    _view->resize(10, 10);
    _view->move(10, 10);
    setView(TroyGLView::FRONT);

    _mirrorLabel->hide();
}

TroyFrame::~TroyFrame() {
    _controller->deleteLater();
    _model->deleteLater();
    _view->deleteLater();
    if (_action != NULL && _ownsAction) {
        _action->deleteLater();
    }
}

void TroyFrame::setView(TroyGLView::ViewOption option) {
    _view->setView(option);
}

void TroyFrame::setAction(RobotAction *action) {
    if (_action != NULL && _ownsAction) {
        _action->deleteLater();
    }
    _action = action;
    if (_action != NULL) {
        _actionLoopCount = _action->loopCount();
        _ownsAction = true;
        _action->setRobot(_model);
        _action->setLoopCount(-1);
        _action->start();
    }
}

RobotAction* TroyFrame::takeAction() {
    if (_action != NULL) {
        RobotAction* action = _action;
        _ownsAction = false;
        _action = NULL;
        action->setRobot(new NullBot());
        action->setLoopCount(_actionLoopCount);
        return action;
    } else {
        return NULL;
    }
}

MotionKeyFrame TroyFrame::getKeyFrame() const {
    MotionKeyFrame keyFrame;
    foreach (int joint, _keyFrame.keys()) {
        if (_keyFrame.contains(remapJoint(joint, _mirrored))) {
            keyFrame.jointValues.append(JointValue(joint, _keyFrame.find(remapJoint(joint, _mirrored)).value()));
        }
    }
    return keyFrame;
}

void TroyFrame::setTroyScale(float percent) {
    //qDebug() << "Scale:" << percent;
    int mirroredHeight = _mirrored ? _mirrorLabel->height() : 0;

    int sizeX = percent * size().width();
    int sizeY = percent * (size().height() - mirroredHeight);

    int offsetX = (size().width() - sizeX) / 2.0;
    int offsetY = (size().height() - sizeY) / 2.0;

    _view->move(offsetX, offsetY + mirroredHeight);
    _view->resize(sizeX, sizeY);

    _troyScale = percent;
}

bool TroyFrame::mirror() const {
    return _mirrored;
}

void TroyFrame::mirror(bool on) {
    _mirrored = on;
    _mirrorLabel->setVisible(_mirrored);
    setTroyScale(_troyScale);
    foreach (int joint, _keyFrame.keys()) {
        _model->setJointAngle(remapJoint(joint, _mirrored), _keyFrame.find(joint).value());
    }
}

void TroyFrame::setJointAngle(int jointId, float theta) {
    _model->setJointAngle(remapJoint(jointId, _mirrored), theta);
    _keyFrame.insert(jointId, theta);
}

void TroyFrame::moveJointBy(int jointId, float delta) {
    _model->moveJointBy(remapJoint(jointId, _mirrored), delta);
    _keyFrame.insert(jointId, _model->jointAngle(jointId));
}

void TroyFrame::startBatch() {
    _model->startBatch();
}

void TroyFrame::endBatch() {
    _model->endBatch();
}

void TroyFrame::loadKeyFrame(MotionKeyFrame keyFrame) {
    startBatch();
    foreach (JointValue joint, keyFrame.jointValues) {
        _keyFrame.insert(joint.first, joint.second);
        _model->setJointAngle(remapJoint(joint.first, _mirrored), joint.second);
    }
    endBatch();
}

void TroyFrame::resizeEvent(QResizeEvent *event) {
    setTroyScale(_troyScale);
}

int TroyFrame::remapJoint(int jointId, bool on) {
    if (on) {
        switch (jointId) {
        case 0:
            return 16;
            break;
        case 1:
            return 17;
            break;
        case 8:
            return 24;
            break;
        case 9:
            return 25;
            break;
        case 16:
            return 0;
            break;
        case 17:
            return 1;
            break;
        case 24:
            return 8;
            break;
        case 25:
            return 9;
            break;
        }
        return jointId;
    } else {
        return jointId;
    }
}
